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ABSTRACT 


This  thesis  focuses  on  evaluating  the  measurement  errors  in  the  gimbal  system  of 
the  SUAV  autonomous  aircraft  developed  at  NPS.  These  measurements  are  used  by  the 
vision  based  target  position  estimation  system  developed  at  NPS.  Analysis  of  the  errors 
inherent  in  these  measurements  will  help  direct  future  investment  in  better  sensors  to 
improve  the  estimation  system’s  performance. 


v 


THIS  PAGE  INTENTIONALLY  LEFT  BLANK 


vi 


TABLE  OF  CONTENTS 


I.  INTRODUCTION . 1 

A.  BACKGROUND . 1 

B.  SUAV . 2 

C.  CONTROL  SYSTEM . 4 

1.  Piccolo  Autopilot . 5 

2.  NPS  Autopilot . 6 

D.  CAMERA . 6 

1.  Gimbal  Pan/Tilt  Unit . 6 

2.  PerceptiVU . 7 

II.  PROBLEM  FORMULATION . 9 

A.  PROBLEM . 9 

1.  Gimbal  Angle  Errors . 9 

B.  AVAILABLE  DATA . 9 

1.  Loading  Data . 11 

2.  Geodetic  to  ECEF . 12 

3.  ECEF  to  LTP . 13 

4.  Target  Position  in  Camera  Frame . 14 

5.  Pixel  Calculation . 17 

C.  ANALYSIS  OF  THE  RESULTS.  CONCLUSION . 19 

III.  SYSTEM  DEVELOPMENT . 23 

A.  OVERVIEW . 23 

B.  COMPUTING  FOUND  ANGLES . 23 

C.  COMPUTING  IDEAL  ANGLES . 26 

1.  Simulink  Portion . 27 

2.  MATLAB  Portion . 27 

IV.  RESULTS . 31 

A.  IDEAL  ANGLES . 31 

B.  COMPARING  RESULTS . 32 

1.  Gimbal  Angles . 32 

C.  RECOMMENDATION . 34 

APPENDIX  A:  SIMULINK  BLOCK  DIAGRAMS . 35 

A.  LOAD  DATA  BLOCK . 35 

B.  LLA  TO  ECEF  BLOCK . 36 

C.  ECEF  TO  LTP  BLOCK . 37 

D.  TARGET  POSITION  IN  CAMERA  FRAME  BLOCK . 38 

E.  COMPUTE  PIXEL  BLOCK . 38 

F.  COMPUTE  FOUND  ANGLES  BLOCK . 39 

APPENDIX  B:  MATLAB  CODE . 41 

A.  FMINUNC  MATLAB  HELP  FILE . 41 

vii 


B.  GIMBAL.M . 42 

C.  GIMBALOPTIMIZATION.M . 43 

LIST  OF  REFERENCES . 45 

INITIAL  DISTRIBUTION  LIST . 47 


viii 


LIST  OF  FIGURES 


Figure  1.  VBTT  Architecture . 2 

Figure  2.  Gimbaled  Camera . 3 

Figure  3.  SUAV . 3 

Figure  4.  SUAV  and  Camera . 4 

Figure  5.  Piccolo  Avionics  Payload . 5 

Figure  6.  Current  Ground  Station . 5 

Figure  7.  Screen  Shot  from  PerceptiVU . 8 

Figure  8.  Entire  Simulink  Model . 10 

Figure  9.  Geodetic  Coordinate  System . 12 

Figure  10.  ECEF  Coordinate  System . 13 

Figure  1 1 .  Visual  Representation  of  e  0LOS  T . 15 

Figure  12.  Visual  representation  of  e  y/LOS  T . 15 

F igure  13.  T arget  Proj  ection  onto  the  Camera  F rame . 17 

Figure  14.  Given  vs.  Found  Pixel  Location  of  the  Target  in  Camera  Frame . 19 

Figure  15.  Plot  of  camera  window . 20 

Figure  16.  True  Pixel  Standard  Deviation . 21 

Figure  17.  Estimated  Pixel  Standard  Deviation . 21 

Figure  18.  aF  vs.  aG . 25 

Figure  19.  J3F  vs.  jBg . 26 

Figure  20.  FMINUNC  Process  Wide  View . 29 

Figure  21.  FMINUNC  Process  Zoom . 30 

Figure  22.  a,  vs.  aF  vs.  aG . 31 

Figure  23.  J3,  vs.  J3F  vs.  J3G . 32 

Figure  24.  Difference  Between  Ideal  a,  and  Given  aG . 33 

Figure  25.  Difference  Between  Ideal  /?,  and  Given  fiG . 33 

Figure  26.  Load  Data  Block . 35 

Figure  27.  LLA  to  ECEF  Block . 36 

Figure  28.  Implementation  of  ECEF  Equations  in  LLA  to  ECEF  Block . 36 

Figure  29.  ECEF  to  LTP  Block . 37 

Figure  30.  Target  Position  in  Camera  Frame  Block . 38 

Figure  3 1 .  Compute  Estimated  Pixel  Block . 38 

Figure  32.  Compute  Pixel  Size  Block . 39 

Figure  33.  Compute  Found  Angles  Block . 39 


IX 


THIS  PAGE  INTENTIONALLY  LEFT  BLANK 


x 


LIST  OF  TABLES 


Table  1.  Data  input  into  simulink  diagram. 


11 


xi 


THIS  PAGE  INTENTIONALLY  LEFT  BLANK 


ACKNOWLEDGMENTS 


I  would  like  to  thank  Dr.  Isaac  Kaminer  and  Dr.  Vladimir  Dobrokhodov  for  their 
endless  patience  in  assisting  me  to  accomplish  this  task.  There  were  many  times  when  I 
knew  that  the  questions  I  was  asking  were  trivial  to  them  but  they  took  the  time  to  help 
me  understand  what  I  needed  to  do  to  get  the  job  done.  I  would  also  like  to  thank  them 
for  their  friendship  that  I  believe  we  have  built  along  the  way.  I  thank  them  for  giving 
me  this  challenge  that  will  make  others  down  the  road  seem  simple.  I  will  always 
remember  them  and  look  to  the  example  they  set  of  what  hard  work  really  is  when  I  times 
are  hard.  Both  of  these  men  have  done  a  lot  with  their  lives  and  I  hope  I  can  accomplish 
as  much  some  day. 

I  would  also  like  to  thank  my  wife  who  had  to  almost  live  without  me  for  the 
months  it  took  me  to  finish  this  thesis.  The  time  I  wasn’t  there  for  her  was  hard  on  her 
but  she  persevered  through  it  very  well  and  I  thank  her  for  being  strong  for  me. 

I  would  also  like  to  thank  Todd  Trago  and  Tom  Brashear  for  their  help.  Whether 
it  was  just  being  there  to  chat  with  and  escape  the  pressure  that  a  thesis  puts  on  your 
shoulders  or  being  there  to  answer  my  usually  stupid  questions  I  thank  them.  I  know 
Todd  will  have  a  great  career  and  I  am  positive  Tom  will  continue  his  success. 


THIS  PAGE  INTENTIONALLY  LEFT  BLANK 


xiv 


I. 


INTRODUCTION 


A.  BACKGROUND 

The  purpose  of  the  Small  Unmanned  Aerial  Vehicle  (SUAV)  is  to  provide  video 
and  target  position  estimations  to  the  Tactical  Network  Topology  (TNT)  Experiment 
being  conducted  by  the  Naval  Postgraduate  School.  “TNT  is  an  integrated  program  of 
quarterly  field  experiments  that  develop  and  demonstrate  new  technologies  to  support 
near  term  needs  of  the  war  fighter”  [4],  The  experiment  focuses  mainly  on  “wireless 
networks,  autonomous  vehicles,  sensor  networks,  situational  awareness  and  target 
tracking  and  identification”  [4], 

The  SUAV  is  just  one  component  in  the  TNT  Experiment.  The  SUAV  provides 
Vision-Based  Target  Tracking  (VBTT)  which  proves  detailed  reconnaissance  and 
simultaneous  imagery,  see  Figure  (1).  The  detailed  reconnaissance  includes  target 
position  estimation.  Targets  can  be  acquired  using  an  onboard  gimbaled  camera.  The 
angles  of  the  camera  (pointed  toward  a  target)  with  respect  to  the  body  of  the  SUAV 
along  with  telemetry  data  from  an  onboard  Inertial  Navigation  System  (INS)  allows  us  to 
estimate  position  of  a  target.  Along  with  live  video  this  target  position  estimation  is  the 
main  mission  of  the  SUAV. 
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Figure  1.  VBTT  Architecture 


B.  SUAV 

The  SUAV  is  like  the  Predator  UAV  although  it  is  a  fraction  of  the  size  and  price. 
Its  small  size,  light  weight,  ruggedness  and  endurance  provide  a  very  useful  tool  on  the 
battlefield. 

Its  primary  payload  is  a  gimbaled  camera,  Figure  (2),  that  has  an  independent 
control  system,  manually  or  autonomously  controllable  from  the  ground. 
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Figure  2.  Gimbaled  Camera. 

The  SUAV  itself,  Figures  (3)  and  (4),  is  a  modified  hobby  aircraft  called  the 
Senior  Telemaster.  It  comes  in  at  2.5  meters  across  the  wings  and  at  a  weight  of  8kg.  The 
SUAV  is  powered  by  a  23  cm3  gasoline  powered  two  stroke  engine  that,  when  coupled 
with  its  1500  cm3  gas  tank,  has  the  ability  to  loiter  for  around  three  hours. 

The  SUAV  carries  the  Piccolo  avionics  package  which  includes  the  INS;  Piccolo 
allows  the  SUAV  to  be  autonomous. 


Figure  3.  SUAV. 
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Figure  4.  SUAV  and  Camera. 


C.  CONTROL  SYSTEM 

The  SUAV  is  equipped  with  an  onboard  integrated  GPS/INS  system  which  also 
transmits  position,  velocity,  acceleration,  Euler  rates  and  angles  to  the  ground. 
Computers  on  the  ground,  using  this  supplied  telemetry  data  can  then  use  it  to  control  the 
flight  of  the  aircraft  and  make  the  SUAV  navigation  autonomous.  Two  types  of  autopilot 
control  are  used  on  the  SUAV,  The  Piccolo  Autopilot  system  and  the  NPS  Autopilot. 
Both  use  the  telemetry  data  which  comes  from  the  Piccolo  avionics  payload  onboard  the 
aircraft,  Figure  (5).  Rate  gyros,  accelerometers,  a  GPS  receiver  and  a  pressure  sensor  are 
all  part  of  the  avionics  payload.  The  payload  communicates  via  a  900  MHz  radio  link  to 
the  ground  station,  Figure  (6). 
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Figure  5.  Piccolo  Avionics  Payload. 


Figure  6.  Current  Ground  Station. 

1.  Piccolo  Autopilot 

Piccolo  was  developed  by  Cloud  Cap  Technology.  It  has  a  user  interface 
compatible  with  Windows  run  on  a  laptop  computer  in  the  field.  The  user  interface 
displays  a  map  of  the  local  area  and  allows  the  user  to  manipulate  waypoints  while  the 
SUAV  is  in  flight  and  view  telemetry  data  being  sent  from  the  aircraft.  This  data  is  saved 
in  a  file  as  it  arrives. 
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The  SIJAV  can  be  put  into  autonomous  mode  or  be  switched  into  manual  mode  at 
the  press  of  a  button.  When  in  autonomous  mode  Piccolo  controls  the  aircraft  by  flying 
the  aircraft  to  waypoints.  However,  when  a  target  is  locked,  NPS  Autopilot  is  engaged 
and  sends  commands  through  the  Piccolo  software  to  the  SUAV. 


2.  NPS  Autopilot 

NPS  Autopilot  was  developed  at  the  Naval  Postgraduate  School.  It  is  an  autopilot 
system  used  once  a  target  is  locked  to  fly  the  SUAV  around  the  target  around  the 
trajectory  that  converges  to  a  circle.  Flying  the  SUAV  around  the  target  in  a  circle 
enables  calculations  to  be  made  to  estimate  the  range  to  the  target,  Equation(O.l). 


Where  $  is  the  angular  rate  of  the  LOS  to  the  target,  v  is  the  velocity  of  the 
SUAV  and  p  is  the  estimated  range  from  the  SUAV  to  the  target.  With  the  estimated 
range  to  the  target,  the  location  of  the  SUAV,  the  Euler  angles  of  the  aircraft  as  well  as 
the  gimbal  angles  all  known,  the  target  position  can  be  estimated. 

D.  CAMERA 

The  camera,  attached  to  a  gimbal  pan/tilt  unit,  is  the  primary  payload  of  the 
SUAV.  Video  data  is  sent  to  the  ground  from  the  UAV  through  a  2.4  GHz  omni¬ 
directional  antenna  and  received  through  a  high  gain  tracking  antenna.  Once  received  the 
video  information  is  time  stamped  and  displayed  through  PerceptiVU  on  a  computer 
video  screen.  PerceptiVU  is  the  program  used  to  lock  onto  a  target. 

1.  Gimbal  Pan/Tilt  Unit 

While  searching  for  a  target  the  Gimbal  Pan/Tilt  Unit  is  under  manual  control 
from  the  ground.  While  in  manual  mode  the  user  observes  real-time  video  through  a 
computer  screen  and  moves  the  gimbal  with  a  computer  joystick.  The  range  of  the  pan/tilt 
unit  is  90  degrees  tilt  (strait  ahead  to  directly  at  the  ground)  and  360  degrees  pan.  The 
movement  of  the  joystick  is  converted  into  commands  for  the  gimbal.  Gimbal  commands 
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are  sent  via  one  way  communication  to  the  SUAV  through  a  900  MHz  radio  link.  The 
gimbal  is  under  autonomous  control  when  locked  on  a  target. 

2.  PerceptiVU 

PerceptiVU  is  image  tracking  software.  Developed  by  PerveptiVU  inc.  this  is  the 
software  we  use  to  lock  onto  a  target  and  stay  locked  on.  A  target  lock  is  achieved  by  the 
user  manning  the  joystick.  The  user  initially  moves  the  gimbal  so  that  the  target  is  in  the 
PerceptiVU  video  screen  and  presses  the  trigger  on  the  joystick.  Once  locked  onto  a 
target,  information  provided  by  PerceptiVU  software  is  used  to  control  the  gimbal 
autonomously.  While  locked  on,  PerceptiVU  outputs  the  location  of  the  target  in  the 
camera  frame.  The  output  is  given  in  pixels.  The  pixel  where  the  target  sits  in  the 
horizontal  direction  in  the  camera  frame  is  called  the  “U”  pixel.  The  vertical  pixel  is  the 
“V”  pixel.  The  PerceptiVU  frame  has  320  pixels  horizontal  and  240  pixels  vertical 
resolution. 

During  the  screen  shot  in  Figure  (7)  PerceptiVU  would  output  pixel  values  of 
+30V  and  +40U  for  example.  The  gimbal  commands  while  locked  onto  a  target  are 
based  on  this  U,  V  data.  The  pitch  rate  commands  to  the  gimbal  depend  on  the  V  pixel. 
The  yaw  rate  commands  to  the  gimbal  depend  on  the  U  pixel.  These  commands  are 
meant  to  keep  the  target  in  the  center  of  the  camera  frame. 

When  the  user  wants  to  disengage  and  regain  manual  control  of  the  gimbal  the 
trigger  is  pulled  again  and  the  target  is  disengaged. 
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Figure  7.  Screen  Shot  from  PerceptiVU. 
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II.  PROBLEM  FORMULATION 


A.  PROBLEM 

Errors  in  the  target  position  estimation  range  on  average  from  20  to  50  meters. 
Some  of  these  errors  are  attributed  to  imperfect  measurements  of  Euler  angles  by  Piccolo 
INS  and  of  gimbal  angles  by  NPS  ground  station.  The  purpose  of  this  thesis  is  to  use 
available  flight  test  data  to  quantify  the  errors  in  the  Gimbal  angle  measurement. 

The  approach  adopted  in  this  thesis  is  twofold: 

•  Using  known  target  and  UAY  positions,  use  Euler  and  gimbal  angle 
measurements  to  determine  where  the  target  would  appear  in  the  camera 
frame.  Compare  computed  Target  position  with  actual  taken  during  the 
flight  test 

•  Using  PerceptiVU  measurements,  determine  the  gimbal  angles  that  would 
result  in  these  measurements.  Compare  these  resultant  angles  with  flight 
test  data. 

1.  Gimbal  Angle  Errors 

The  gimbal  data  likely  represents  a  source  of  errors.  In  this  work  it  is  assumed 
that  commands  sent  to  the  gimbal  are  executed  immediately,  therefore  output  position  of 
the  camera  attached  to  it  is  equal  to  the  input  command.  However,  extensive 
experimentation  shows  that  this  in  not  the  case.  Several  factors  distort  this  input-output 
relation: 

•  Calibration  of  the  gimbal; 

•  Time  delay  introduced  by  wireless  RF  link; 

•  Noise  due  to  the  atmospheric  and  engine  noise. 

B.  AVAILABLE  DATA 

To  address  these  problems  we  will  use  the  following  flight  test  data: 

•  Position  of  the  target  in  the  camera  frame  provided  by  PerceptiVU; 

•  The  location  of  the  SUAY  in  geodetic  coordinates; 

•  The  location  of  the  target  in  geodetic  coordinates; 

•  The  Euler  angles  of  the  aircraft  in  radians; 

•  The  gimbal  angles  in  radians; 
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•  Video  recording  from  the  camera  on  the  specific  flight. 

From  this  data  we  can  calculate  orientation  of  the  camera.  Using  this  orientation 
and  the  known  location  of  the  target  we  can  estimate  the  location  of  the  target  in  the 
camera  frame  and  compare  it  to  the  true  data  taken  from  PerceptiVU.  The  difference  in 
this  location  of  the  target  in  the  camera  frame  will  provide  us  insight  into  the  errors  in  the 
gimbal  angles  of  the  SUAV.  To  calculate  the  estimated  pixel  location  MATLAB  7.0 
Simulink  was  used.  A  Simulink  implementation  of  the  approaches  outlined  in  II.A  is 
presented  in  Figure  (8). 


[CAMERA  POP 


Compute  Ideal  Gimble  and  Euler  Angles  Scopes 


Figure  8.  Entire  Simulink  Model 
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1.  Loading  Data 

Loading  the  data  is  the  first  block  of  the  Simulink  diagram.  For  each  flight  of  the 
SIJAV  its  telemetry,  gimbal  and  PerceptiVU  data  is  saved  versus  real-time  in  the  form  of 
a  MATLAB  .mat  file.  The  data  loaded  at  the  first  step  of  the  Simulink  model,  Figure  (8) 
is  presented  in  Table  1. 


Data  (units) 

Subfile 

Element 

Target  Position  (rad,m) 

N/A 

N/A 

SUAV  Latitude  (rad) 

Telem 

1 

SUAV  Longitude  (rad) 

Telem 

2 

SUAV  Altitude,  above  ground  (m) 

Telem 

3 

SUAV  Roll,  psi,  <pG  (rad) 

Ctrl 

24 

SUAV  Pitch,  theta,  0G  (rad) 

Ctrl 

25 

SUAV  Yaw,  psi,  y/G  (rad) 

Ctrl 

26 

Gimbal  Pitch,  tilt,  aG 

Gmbl 

1 

Gimbal  Yaw,  pan,  J3G 

Gmbl 

2 

Gimbal  Lock 

Gmbl 

3 

PerceptiVU  Ulme  (pixel) 

Gmbl 

4 

PerceptiVU  Vtrue  (pixel) 

Gmbl 

5 

Table  1.  Data  input  into  simulink  diagram. 


The  Simulink  block  that  loads  the  data  can  be  seen  in  Appendix  (A.A). 
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2.  Geodetic  to  ECEF 

Flight  test  data  provides  the  position  of  the  target  and  SUAV  in  geodetic 


coordinates,  Figure  (9).  In  order  to  solve  our  task  we  need  to  transfer  coordinates  into  a 
Local  Tangent  Plane  (LTP).  This  transformation  is  done  in  two  steps:  first,  is  rotation 
from  geodetic  to  ECEF  (Figure  (10));  second  is  to  rotate  from  ECEF  to  LTP. 

First  step  is  accomplished  through  Equation(0.2). 


\j\-£2  sin2  (p 

xe  =  (rx  +  h)cos<pcosX  (0.2) 

ye  =  (ri  +  h)  cos  (p  sin  X 
ze  =  ((1  -  £2)rx  +  h )  sin  cp 

Where: 

•  <p  is  the  Latitude  angle  (rad) 

•  X  is  the  Longitude  angle  (rad) 

•  h  is  the  height  above  sea  level  (m) 

Implementation  of  these  equations  using  Simulink  viewable  in  Appendix  (A.B). 


Figure  9.  Geodetic  Coordinate  System. 


12 


z 


Earth  Centered,  Earth  Fixed  X,  Y,  Z 

PH  Dana  S/17A4 


Figure  10.  ECEF  Coordinate  System. 


3.  ECEF  to  LTP 

LTP  is  a  right  hand  rule  reference  frame  which  has  its  origin  at  a  chosen  spot.  For 
this  case  the  LTP  origin  is  placed  at  the  target  position.  The  North-East-Down  (NED) 
orientation  of  the  LTP  is  assumed.  Equation  (0.3)  shows  the  rotation  matrix  used  to 
transform  ECEF  to  LTP. 


NED 

e 


R  = 


-  sin#?  cos  T 

-sin/t 

-  cos  #?  cos  X 


-  sin#?  sin  T 

cos  X 

-  cos#?  sin  T 


cos  #? 
0 

-sin#? 


(0.3) 


#?  and  X  in  Equation  (0.3)  are  the  Latitude  and  Longitude  in  the  geodetic  frame 
respectively. 


e  NED 

PsUAV  e 


R*  (  PsUAV  P target') 


(0.4) 
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The  position  of  the  SUAV  in  LTP  coordinates ,NED pSUAV  with  components 
[NEDxSUAVNEDySUAV,NEDzSUAV],  is  found  through  Equation  (0.4)  where  epSUAr  is  the 
position  of  the  SUAV  represented  in  ECEF  coordinates,  and  e  pTarget  is  the  location  of  the 
target  in  ECEF  coordinates.  Equation  (0.5)  calculates  the  position  of  the  target  in  LTP 
frame.  Recall,  e pTw%et  is  the  origin  and  should  be  equal  to  [0,0,0]. 


NED  , 


pr  =  R*(e  pT  — e  p  ,) 

-r  Target  e  V  r' Target  -r  targets 


(0.5) 


Implementation  of  this  transformation  using  Simulink  can  be  seen  in  Appendix 

(A.C). 


4.  Target  Position  in  Camera  Frame 

With  the  Target  position  and  SUAV  position  in  LTP  frame  we  can  calculate  the 
range  from  the  SUAV  to  the  Target  in  Equation(0.6). 


P 


-T 


XSUAVJ 


_l_  /NED 


ySUAV ) 


2  .  (NED 

+  (  Z 


SUA  V 


(0.6) 


Consider  Figure  (11)  and  Figure  (12).  Angular  position  of  the  target  in  the 
camera  frame  is  given  by  c  0LOS  T  and  '  >f/L()S  T  . 
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X, 


Figure  11.  Visual  Representation  of  C0LOS  T 


cPt 

V los,  r 

yc 

Figure  12.  Visual  representation  of  '  Ylos,t  • 

These  two  angles  are  computed  using  the  vector  from  the  SUAV  to  the  Target  and 
two  rotation  matrices.  Because  we  let  the  target  position  be  the  origin  of  the  LTP  frame, 
the  vector  from  the  SUAV  to  the  Target  is  - NED pSUAV  ■  The  first  rotation  matrix, 
Equation(0.7),  uses  the  Given  Gimbal  Angles,  where  aG  is  the  given  tilt  gimbal  angle 
and  J3g  is  the  given  gimbal  pan  angle,  from  flight  test  data.  The  second  rotation  matrix, 
Equation(0.8),  uses  the  Given  Euler  Angles  from  Piccolo  telemetry. 


b 

g 


R  = 


cos  aG  cos  J3g 
cos  aG  sin  J3G 
sin  aG 


-sin  PG 
cos  J3G 
0 


-sinaG  cos  J3G 
-  sin  aG  sin  J3G 
cos  aG 


(0.7) 
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cos  if/ q  cos  0G  cos  \f/G  sin  0G  sin  (pG  -  sin  y/G  cos  (pG  cos  y/G  sin  0G  cos  (pG 

nbR=  sin  y/G  cos  0G  sin  y/G  sin  0G  sin  (pG  +  cos  y/G  cos  (pG  sin  y/G  sin  0G  cos  (pG  -  cos  y/G  sin  (pG 
-  sin  0G  cos  0G  sin  <pG  cos  0G  cos  (pG 

(0.8) 

Equation(0.9)  rotates  the  vector  from  the  SUAV  to  the  Target,  ~NED pSUAV ,  around 
the  flight  test  data  gimbal  angles  to  give  us  the  position  of  the  target  in  the  body 
frame, b  pT . 

bpT=bgR*(-NED  pSUAV)  (0.9) 

Equation(O.lO)  rotates  the  position  of  the  target  in  the  body  frame  to  the  position 
in  the  camera  frame,  c  pT  . 

’ Pt  =:  R*b  Pt  (0.10) 

Position  of  the  Target  in  the  camera  frame, c pT  ,  has  components  [cxT,cyT,czT], 
These  components  are  used  in  the  computation  of  the  tilt  angle  to  the  target  in  the  camera 
frame,  c  0LOS  T ,  in  Equation  (0.11)  and  Figure  (11).  The  pan  angle  to  the  target  in  the 

camera  frame,  cy/LOS  T ,  is  found  in  Equation  (0.12)  and  Figure  (12). 

°  Q LOS, T  =  a  tan(~“)  (0.11) 

xT 

c 

>  =atan(^)  (0.12) 

xT 
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The  angular  location  of  the  target  in  the  camera  frame  and  the  range  are  used  to 
calculate  pixel  number  in  the  camera  frame,  Figure  (13). 


Camera  LOS 


o 

Horizontal  Position  of  Target 
in  Camera  Frame 


Figure  13.  Target  Projection  onto  the  Camera  Frame 

Implementation  of  the  equations  in  this  section  using  Simulink  can  be  seen  in 
Appendix  (A.D). 


5.  Pixel  Calculation 

Our  first  task  calls  for  us  to  compare  Vestimate  and  Uestimate  with  Vtrue  and  Utrue.  In 
order  to  compute  Vestimate  and  Uestimate  we  need  the  following: 

•  Range  from  SUAV  to  Target,  p 

•  Field  of  view  of  the  camera,  0FOV  and  y/FOV 

•  Angular  position  of  the  target  in  the  camera  frame,  C0LOST  and  ipLOS  T 

To  find  0FOV  and  y/FOV  we  set  up  the  camera  and  pointed  it  at  a  wall.  We 

measured  the  distance  from  the  camera  frame  to  the  wall,  d .  One  the  wall  we  measured 
the  width,  y ,  and  height,  z,  of  the  camera’s  field  of  view. 
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(0.13) 


0FOV  =  2  *  a  tan  - 


¥k,v 


=  2  *  a  tan 


(0.14) 


Equation  (0.13)  shows  us  the  field  of  view  (FOV)  in  the  vertical  direction,  0FOV. 
Equation  (0.14)  finds  the  horizontal  FOV,  y/FOV  . 


Once  we  know  the  camera  field  of  view  in  the  horizontal  and  vertical  directions 
we  can  compute  the  pixel  size,  Equation  (0.15)  and  (0.16)  respectively.  Note  that  240  is 
the  number  of  pixels  in  the  PerceptiVU  window  vertically  while  there  are  320  pixels 
across  the  screen  horizontally. 


2/,tan(%) 


v  .  =  ■ 

pixsz 


240 


(0.15) 


2ptm(¥F°v) 


u  .  =  - 

pixsz 


320 


(0.16) 


The  distance  from  the  center  of  the  camera  frame  to  the  target  position  in  the 
camera  frame,  vertically  and  horizontally,  is  calculated  in  Equations  (0.17)  and  (0.18) 
respectively. 


vd,s,  =  Ptan(C  &los,t) 


(0.17) 


dist  P  tan(  P  lost )  (0.18) 

This  distance  divided  by  pixel  size,  Equations  (0.19)  and  (0.20),  gives  us  the 
position  of  the  target  in  the  camera  frame  with  pixels  as  the  units. 
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v  . 

estimate 


v  dist 


pixsz 


(0.19) 


*dist 


pixsz 


(0.20) 


Implementation  of  the  equations  in  this  section  using  Simulink  can  be  seen  in 
Appendix  (A.E). 


C.  ANALYSIS  OF  THE  RESULTS.  CONCLUSION 

The  Kstimate  and  U estimate  Pixels  do  not  match  UP  to  Vlrue  and  Utrue ,  Figure  (14). 
This  offset  in  the  pixel  location  of  the  target  in  the  camera  frame  proves  to  us  that  there  is 
an  error.  This  data  suggests  to  us  that  there  is  a  bias  error  in  the  pan  angle  of  the  camera 
and  a  larger  bias  in  the  tilt  angle  of  the  camera. 


True  and  Estimated  Pixel  Locations 
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Figure  14.  Given  vs.  Found  Pixel  Location  of  the  Target  in  Camera  Frame. 
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The  mean  Vtrue  pixel  number  is  -1.7041  and  the  mean  Utrue  pixel  number  is  7.863 
for  this  set  of  data.  This  is  near  the  center  of  the  camera  frame.  The  Estimated  pixels, 
however,  are  not  near  the  center  of  the  camera  frame.  The  mean  Vestimate  pixel  number  is 

306.2  and  the  mean  Uestimate  pixel  number  is  104.5.  The  difference  between  Vestimate  and 

Vtrue  's  307.9  pixels.  The  difference  between  Uestimate  and  Utrue  is  96.63  pixels.  The  mean 

location  of  the  vertical  pixel  wouldn’t  even  put  the  target  into  the  visible  camera  frame 
since  the  visible  frame  only  goes  to  +120  pixels  vertically,  Figure  (15). 


True  and  Estimated  Pixel  Locations 
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Figure  15.  Plot  of  camera  window. 


The  standard  deviation  of  the  True  Pixels  ,crT  pixel  =[19.42,27.71],  seen  in  Figure 

(16)  is  much  lower  than  the  standard  deviation  of  the  Estimated  Pixels, 
CJESt_PM  =  [65.83,48.37] ,  seen  in  Figure  (17). 
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Standard  Deviation  of  True  Pixels 


Figure  16.  True  Pixel  Standard  Deviation. 


Standard  Deviation  of  Estimated  Pixels 


Horizontal  Pixel  Number,  U 


Figure  17.  Estimated  Pixel  Standard  Deviation. 
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In  conclusion,  the  comparison  between  the  Vestimate/Uestimate  pixels  and  the  True 

pixels  demonstrates  that  there  is  a  biased  constant  error,  that  might  be  modeled  as  a  fixed 
bias.  The  error  comes  from  the  Given  Gimbal  and  Euler  Angles.  Using  these  angles  for 
position  estimation  will  therefore  lead  to  a  coherent  error  in  the  estimated  target  position. 
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III.  SYSTEM  DEVELOPMENT 


A.  OVERVIEW 

Since  there  are  errors  in  the  orientation  of  the  LOS  we  must  find  the  source  of  this 
error.  The  LOS  vector  is  used  in  the  SUAVs  most  important  mission,  estimating  the 
geodetic  location  of  a  target.  With  an  incorrect  LOS  vector  the  NPS  Autopilot 
commands  will  be  off  as  well  causing  the  SUAV  to  circle  around  a  biased  point,  but  not 
the  target’s  position. 

To  find  where  the  most  errors  come  from  in  the  system  we  must  first  calculate 
Gimbal  angles  that  would  match  Vestimate  and  Uestimate  pixel  location  of  the  target  with  Vtrue 

and  Utrue  pixel  location.  If  the  two  pixel  locations  match,  we  know  the  LOS  from  the 
UAV  to  the  target  is  correct  and  a  more  accurate  target  position  is  being  estimated.  For 
every  time  step  the  Ideal  Gimbal  Angles  ( a;  and  f51 )  will  be  computed  so  that  the 
estimated  and  true  pixels  equal  to  one  another. 

The  “Ideal”  gimbal  angles  will  be  computed  and  compared  with  the  gimbal  angles 
that  we  have  from  the  flight  test  data,  Given  angles.  The  difference  between  the  Given 
and  Ideal  Gimbal  Angles  will  show  us  the  error. 


B.  COMPUTING  FOUND  ANGLES 

The  Found  Angles  (aF ,  (3r )  are  those  angles  that  will  direct  the  camera  LOS  to 

point  directly  toward  the  target.  Transformation  (0.21)  is  used  to  rotate  a  unity  vector  of 
the  LOS  in  camera  frame  ([1,0,0])  to  inertial  frame  ([xLOS ; yLOS ; zL()S  ])  by  using  three 
consecutive  rotations: 

•  gcR-  from  Camera  to  Gimbal. 

•  bgR-  from  Gimbal  to  Body. 

•  "hR  -  from  Body  to  Inertial. 
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(0.21) 


XLOS 

V 

y  LOS 

=:  KR‘R 

0 

_  Z LOS  _ 

0 

The  rotation  matrix  gc  R  ,  which  contains  the  angles  found  later  in  equations  (0.26) 

and  (0.27),  is  removed  from  this  part  of  the  process,  since  those  angles  are  not  yet  know, 
and  is  re-introduced  when  solving  for  the  Ideal  Angles  in  part  C  of  this  section. 

Found  Gimbal  Angles  are  computed  while  using  the  Given  Euler  Angles  in  the 
nbR  rotation  matrix  of  Equation(0.22). 


XLOS 

"f 

bnR 

yLOS 

=SR 

0 

_  Z LOS  _ 

0 

(0.22) 


Equation  (0.23)  through  (0.25)  calculate  Found  Gimbal  Angles.  Since  the  gimbal 
only  uses  two  angles  and  we  have  three  known  values  we  can  solve  Equation(0.23). 


X 

cos  PF  cos  aF 

7 

= 

sin  J3f  cos  aF 

Z 

-sin«F 

(0.23) 
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Simplified  out  we  can  solve  for  the  Found  aF ,  Equation  (0.24),  and  Found  J3F , 
Equation  (0.25). 


aF  =  —a  sin(z) 

(0.24) 

y 

/3f  =  a  tan(— ) 

(0.25) 

x 


The  Found  Angles  for  the  gimbal,  aF  and  /3F,  are  shown  in  Figure  (18)  and 
Figure  (19). 


Gimbal  Theta  Angles 
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Gimbal  Psi  Angles 
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Figure  19.  J3F  vs.  fiG. 


Notice  that  in  most  of  the  Found  Angles  there  is  a  very  noticeable,  almost 
constant,  difference  from  the  given  data.  Although  the  Found  Angles  are  not  the  Ideal 
Angles,  they  still  give  us  an  initial  idea  of  where  the  errors  are  occurring  in  the  system. 
Gimbal  a  and  J3  angles  both  appear  to  follow  the  same  trend  but  with  a  bias  between 
them.  Computing  the  Ideal  Angle  will  give  us  the  final  angles  we  are  looking  for  and  we 
can  draw  our  conclusions  from  there. 


Implementation  of  this  section  using  Simulink  is  in  Appendix  (A.F). 


C.  COMPUTING  IDEAL  ANGLES 

The  Found  Angle  is  calculated  to  give  us  a  decent  initial  condition  to  feed  into  the 
iterative  process  used  to  calculate  the  Ideal  Angles.  The  Found  Angle  points  the  camera 
directly  at  the  target.  Looking  at  our  flight  data,  however,  and  the  Vtrue  and  Utrue  pixels  it 

gives  us,  we  know  that  the  camera  is  rarely  pointed  exactly  toward  the  target,  Figure  (7). 
We  know  the  camera  is  slightly  off  the  direct  line  pointing  toward  the  target  and  the 
target  is  moving  around  the  screen.  Computing  the  Ideal  Angles  takes  into  account  this 
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small  difference  between  the  vector  directly  to  the  target  and  to  the  true  center  of  the 
camera  frame. 

1.  Simulink  Portion 

The  angles  between  the  true  camera  vector  and  the  true  pixel  location  can  be 
calculated  as  follows  using  the  calculated  pixel  size  and  range. 

c 0LOSJ  TRUE  =  a tan(^*V^)  (0.26) 

P 

U,  *u  . 

Y los, t  true  ^  tan(  )  (0.27) 

P 

Once  these  true  LOS  to  target  angles  are  computed  a  MATLAB  code  is  used  to 
compute  the  Ideal  Angles. 

The  data  we  have  obtained  from  Simulink  and  will  use  to  find  the  Ideal  Angles 
are: 

•  Given  Euler  Angles  (bg.mat) 

•  Given  and  Found  Gimbal  Angles  (g  g.mat  and  g_f.mat) 

•  SU A V  to  Target  Vector,  ~NEDpSUAV  (los.mat) 

•  True  LOS  to  True  Target  Position  Angles, 1 0LOS  T  TRUE  ,cy/ws  T  TRUE  (l.mat) 
Parenthesis  indicates  the  file  name  that  this  data  is  saved  to  after  each  run  of  the 

model. 


2.  MATLAB  Portion 

The  data  obtained  in  Simulink  is  saved  and  run  through  a  MATLAB  function  to 
calculate  the  set  of  Ideal  Angles.  The  .m  file  called  gimbal.m,  Appendix  (B.B),  is  used  to 
calculate  the  Ideal  Gimbal  Angles. 

The  FMINUNC  command,  Appendix  (B.A,C),  run  in  MATLAB  calls  upon  the 
gimbal.m  file.  The  FMINUNC  function  iterates  through  combinations  of  angles  until  the 
condition  of  the  FMINUNC  command  is  satisfied,  Equation(0.28).  When  the  condition 
of  FMINUNC  is  satisfied,  the  LOS  vector  and  the  vector  [1;0;0]  rotated  through  the  three 
rotation  matrices  are  equal. 
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F  =  norm(\ 


) 


(0.28) 


- 1 

§ 

_ 1 

"f 

y  los 

0 

_  Z LOS  _ 

0 

When  searching  for  the  Ideal  Gimbal  Angles,  the  angles  inside  of  the  f  R  rotation 
matrix  are  the  ones  that  will  be  changed  until  the  condition  is  satisfied.  The  angles  inside 
bnR  are  the  Given  Euler  Angles  from  Piccolo.  The  angles  inside  c  R  were  calculated 
with  Equations  (0.26)  and  (0.27). 

Once  the  FMINUNC  function’s  condition  is  satisfied  it  will  output  the  Ideal 
Angles.  As  was  said  earlier  the  Found  Angles  are  used  as  initial  conditions  or  the  starting 
point  in  finding  the  Ideal  Angles.  It  was  decided  that  the  Found  angles  were  satisfactory 
as  an  initial  condition,  because  the  actual  position  of  the  target  is  never  too  far  away  from 
the  center  of  the  camera  frame,  where  the  Found  Angles  are  pointing. 

Figure  (20)  is  included  to  show  visually  how  the  FMINUNC  function  converges 
at  its  solution.  At  a  specific  time  in  the  data  set  the  FMINUNC  function  runs  through 
gimbal  angles  until  the  minimum  of  the  function  is  reached. 
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Figure  20.  FMINUNC  Process  Wide  View 

Looking  closer  towards  the  minimum  of  Figure  (20)  we  get  Figure  (21). 
Readings  from  the  figure  gives  us  a  tilt  (a7 )  value  at  the  minimum  found  between  .885 

and  .89,  near  .8855.  The  pan  (/?7 )  value  at  the  minimum  is  between  -1.552  and  -1.554, 

near  -1.5535.  The  FMINUNC  function  would  then  output  [.8855,-1.5535]  as  the  answer 
at  this  specific  time.  These  are  the  Ideal  Angle  we  are  looking  for. 
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Norm  Error  of  Tilt  vs  Pan  Angle 


0.895 


Figure  21.  FMINUNC  Process  Zoom. 
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IV.  RESULTS 


IDEAL  ANGLES 

The  FMINUNC  function  gave  us  the  results  illustrated  in  Figures  (22)  and  (23). 


Gimbal  Theta  Angles 


Figure  22.  a,  vs.  aF  vs.  aG . 


Gimbal  Psi  Angles 


Figure  23.  /?,  vs.  fiF  vs.  J3G . 

B.  COMPARING  RESULTS 
1.  Gimbal  Angles 

Upon  visual  inspection  it  follows  that  the  Given  Gimbal  Angles  follow  the  same 
path  as  the  Ideal  Gimbal  Angles  but  are  offset  from  one  another  by  a  certain  bias, 
Figure  (22)  and  Figure  (23).  The  offset  or  bias  are  plotted  in  Figure  (24)  and  Figure  (25). 
This  could  be  attributed  to  errors  in  the  calculation  of  the  Ideal  Gimbal  Angles  or  it  could 
be  caused  by  some  bias  error  in  the  given  data. 
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Figure  24 


Gimbal  Theta  Difference 
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Figure  25.  Difference  Between  Ideal  ft,  and  Given  . 
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It  suggests  that  a  simple  addition  or  gain  in  the  gimbal  command  could  fix  the 
bias  in  the  a  and  /?  angles.  Fixing  the  bias  would  match  the  Given  Angles  to  the  Ideal 
Angles.  Eliminating  the  bias  in  the  Gimbal  Angles  from  the  NPS  ground  station 
readiness/telemetry  would  reduce  the  error  in  the  target  position  estimation. 

C.  RECOMMENDATION 

My  recommendation  to  the  project  is  to  invest  additional  resources  into  the 
following  projects. 

•  A  model  of  the  gimbal  that  will  account  errors  induced  while  sending  data 
to  the  gimbal,  i.e.  time  delay,  gimbal  movement  delays. 

•  Taking  the  bias  between  the  Ideal  Gimbal  Angles  and  the  Given  Gimbal 
Anlges  into  account  when  using  the  Given  Gimbal  Angle  for  position 
estimation. 

•  In  depth  Gimbal  calibration  investigation. 

•  A  more  accurate  measuring  device  onboard  the  SUAV  for  better  readings 
of  its  Euler  Angles. 

It  appears  that  some  changes  can  be  made  in  the  gimbal  loop  that  will  not  raise  the 
price  of  the  SUAV.  In  the  future,  research  should  be  done  on  modeling  the  gimbal 
dynamics  and  looking  deeper  into  the  bias  found  there.  If  that  does  not  solve  the 
problem,  then  a  better  avionics  package  onboard  the  SUAV  might  solve  our  problems. 


34 


APPENDIX  A:  SIMULINK  BLOCK  DIAGRAMS 


A.  LOAD  DATA  BLOCK 


Select  T arget  Lock  T arget  Lock 

(element  3) 


Figure  26.  Load  Data  Block. 
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B, 


LLA  TO  ECEF  BLOCK 


equator  radius 


Scopel 


Figure  27.  LLA  to  ECEF  Block. 


Figure  28.  Implementation  of  ECEF  Equations  in  LLA  to  ECEF  Block. 
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c 


ECEF  TO  LTP  BLOCK 


ECEF  to  LTP 


The  MATLAB  code  within  the  Embedded  MATLAB  Function  of  Figure  (39). 


function  Re2u  =  f cn (latT, longT, hT) 

%  This  block  supports  an  embeddable  subset  of  the  MATLAB  language. 
%  See  the  help  menu  for  details. 


Re2u= [cos (latT) *cos (longT)  cos (latT) *sin (longT)  sin(latT); 
-sin (longT)  cos (longT)  0; 

-sin (latT) *cos (longT)  -sin (latT) *sin (longT)  cos  (latT) ] ; 
RNED= [ 0  0  1 ; 0  1  0;-l  0  0] ; 

Re  2  u=RNE  D  *  Re  2  u ; 
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D 


TARGET  POSITION  IN  CAMERA  FRAME  BLOCK 


Figure  30.  Target  Position  in  Camera  Frame  Block. 
E.  COMPUTE  PIXEL  BLOCK 


T arget  Lock 


Figure  31.  Compute  Estimated  Pixel  Block. 
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Range  UAV  to  Target 
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Pixels  in  V  direction 
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Divide4 


->CZ3 
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Figure  32.  Compute  Pixel  Size  Block. 
F.  COMPUTE  FOUND  ANGLES  BLOCK 


ICOMPUTE  FOUND  GIMBAL  AND  EULER  ANGLES 


no» 

g  J 


Found  Gimble 
Angles 


I  lg_gJ» 
g_fi 


Given  Gimble 
Angles 


Figure  33.  Compute  Found  Angles  Block. 
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APPENDIX  B:  MATLAB  CODE 


FMINUNC  MATLAB  HELP  FILE 


FMINUNC  finds  the  minimum  of  a  function  of  several  variables. 

X=FMINUNC ( FUN, XO )  starts  at  XO  and  attempts  to  find  a  local  minimizer  X 
of  the  function  FUN.  FUN  accepts  input  X  and  returns  a  scalar  function 
value  F  evaluated  at  X.  XO  can  be  a  scalar,  vector  or  matrix. 

X=FMINUNC (FUN, XO , OPTIONS )  minimizes  with  the  default  optimization 
parameters  replaced  by  values  in  the  structure  OPTIONS,  an  argument 
created  with  the  OPTIMSET  function.  See  OPTIMSET  for  details.  Used 
options  are  Display,  TolX,  TolFun,  DerivativeCheck,  Diagnostics, 

FunValCheck  GradOb j ,  HessPattern,  Hessian,  HessMult,  HessUpdate, 
InitialHessType,  InitialHessMatrix,  MaxFunEvals,  Maxlter, 

Dif fMinChange  and  Dif fMaxChange,  LargeScale,  MaxPCGIter, 

PrecondBandWidth,  TolPCG,  TypicalX.  Use  the  GradObj  option  to  specify 
that  FUN  also  returns  a  second  output  argument  G  that  is  the  partial 
derivatives  of  the  function  df/dX,  at  the  point  X.  Use  the  Hessian 
option  to  specify  that  FUN  also  returns  a  third  output  argument  H  that 
is  the  2nd  partial  derivatives  of  the  function  (the  Hessian)  at  the 
point  X.  The  Hessian  is  only  used  by  the  large-scale  method,  not  the 
line-search  method. 

[X, FVAL] =FMINUNC (FUN, XO , ... )  returns  the  value  of  the  objective 
function  FUN  at  the  solution  X. 

[X, FVAL, EXITFLAG] =FMINUNC (FUN, XO, ... )  returns  an  EXITFLAG  that  describes 
the  exit  condition  of  FMINUNC.  Possible  values  of  EXITFLAG  and  the 
corresponding  exit  conditions  are 

1  FMINUNC  converged  to  a  solution  X. 

2  Change  in  X  smaller  than  the  specified  tolerance. 

3  Change  in  the  objective  function  value  smaller  than  the  specified 

tolerance  (only  occurs  in  the  large-scale  method) . 

0  Maximum  number  of  function  evaluations  or  iterations  reached. 

-1  Algorithm  terminated  by  the  output  function. 

-2  Line  search  cannot  sufficiently  decrease  the  objective  function  along 

the  current  search  direction  (only  occurs  in  the  medium-scale  method) . 

[X, FVAL, EXITFLAG, OUTPUT] =FMINUNC (FUN, XO, ... )  returns  a  structure  OUTPUT 
with  the  number  of  iterations  taken  in  OUTPUT . iterations ,  the  number 
of  function  evaluations  in  OUTPUT . funcCount ,  the  algorithm  used  in 
OUTPUT . algorithm,  the  number  of  CG  iterations  (if  used)  in 
OUTPUT . cgiterations ,  the  first-order  optimality  (if  used)  in 
OUTPUT . firstorderopt ,  and  the  exit  message  in  OUTPUT . message . 

[X, FVAL, EXITFLAG, OUTPUT, GRAD] =FMINUNC (FUN, XO, ... )  returns  the  value 
of  the  gradient  of  FUN  at  the  solution  X. 

[X, FVAL, EXITFLAG, OUTPUT, GRAD, HESSIAN] =FMINUNC (FUN, XO, ...)  returns  the 
value  of  the  Hessian  of  the  objective  function  FUN  at  the  solution  X. 

Examples 

FUN  can  be  specified  using  @ : 

X  =  fminunc ( @myfun, 2 ) 

where  MYFUN  is  a  MATLAB  function  such  as : 

function  F  =  myfun (x) 

F  =  sin(x)  +  3; 

To  minimize  this  function  with  the  gradient  provided,  modify 
the  MYFUN  so  the  gradient  is  the  second  output  argument: 
function  [f,g]=  myfun (x) 
f  =  sin (x)  +  3 ; 

g  =  cos (x) ; 

and  indicate  the  gradient  value  is  available  by  creating  an  options 


structure  with  OPTIONS . GradObj  set  to  'on'  (using  OPTIMSET) : 
options  =  optimset (' GradObj on ')  ; 
x  =  fminunc (' myfun '  ,  4 , options ) ; 

FUN  can  also  be  an  anonymous  function: 

x  =  fminunc (@(x)  5*x ( 1 ) A2  +  x(2)A2,[5;l]) 

If  FUN  is  parameterized,  you  can  use  anonymous  functions  to  capture  the 
problem- 

dependent  parameters.  Suppose  you  want  to  minimize  the  objective  given  in  the 
function  MYFUN,  which  is  parameterized  by  its  second  argument  A.  Here  MYFUN  is 
an  M-file  function  such  as 

function  [f,g]  =  myfun (x, a) 

f  =  a*x(l)A2  +  2*x(l)*x(2)  +  x(2)A2;  %  function 

g  =  [2*a*x(l)  +  2*x(2)  %  gradient 

2 *x ( 1 )  +  2*x  (2 ) ] ; 

To  optimize  for  a  specific  value  of  A,  first  assign  the  value  to  A.  Then 
create  a  one-argument  anonymous  function  that  captures  that  value  of  A 
and  calls  MYFUN  with  two  arguments.  Finally,  pass  this  anonymous  function 
to  FMINUNC: 

a  =  3;  %  define  parameter  first 

options  =  optimset (' GradObj on ') ;  %  indicate  gradient  is  provided 
x  =  fminunc (@ (x)  myfun (x, a) ,[ 1 ; 1 ], options ) 


See  also  optimset,  fminsearch,  fminbnd,  fmincon,  @,  inline. 


B.  GIMBAL.M 

function  [F]  =  gimbal (x) 

%  this  block  sets  up  the  function  to  be  used  in  the  optimization 
%finding  the  best  tilt  and  pan  angles  to  show  gimble  command  error 

load  los; 
load  b_g; 
load  1; 
load  g_f; 
global  i 

LOS_v  =  [los (2,i) ;los (3,i) ; los (4, i)  ]  ; 

Rb2LTP= [cos (b_g (4 , i) ) *cos (b_g (3, i) )  sin (b_g (4 , i) ) *cos  (b_g  (3, i) ) 

sin (b_g (3, i) ) ; cos (b_g (4, i) ) *sin (b_g (3, i) ) *sin (b_g (2, i) ) - 
(sin (b_g ( 4 , i ) ) *cos (b_g (2 , i ) ) ) 

sin (b_g (4, i) ) *sin (b_g (3, i) ) *sin (b_g (2, i) ) + (cos (b_g (4, i) ) *cos (b_g (2, i) ) ) 
cos (b_g (3, i) ) *sin (b_g (2, i) ) ; cos (b_g (4, i) ) *sin (b_g (3, i) ) *cos (b_g (2, i) ) + (sin (b_g (4, i 
) ) *sin (b_g (2, i) ) )  sin  (b_g (4, i) ) *sin (b_g (3, i) ) *cos (b_g (2,  i)  )  - 

(cos (b_g (4, i) ) *sin (b_g (2, i) ) )  cos (b_g (3, i) ) *cos (b_g (2, i) ) ] . * ; 

Rc2b= [cos (x (1) ) *cos  (x (2) )  cos (x (1) ) *sin (x  (2 ) )  sin (x ( 1 ) ) ; -sin (x (2 ) )  cos (x (2) )  0;- 

sin (x (1) ) *cos (x  (2 ) )  -sin (x (1) ) *sin  (x (2) )  cos (x  (1) ) ]  . 1 ; 

RLOS2c=  [cos  (1  (2,  i)  )*cos(l(3,i)  )  cos  (1  (2,i)  )*sin(l(3,i)  )  sin(l(2,i)  )  ;-sin(l(3,i)  ) 
cos(l(3,i))  0; -sin (1 (2,i) ) *cos (1 (3, i) )  -sin (1 (2, i) ) *sin (1 (3, i) )  cos (1 (2, i) ) ] . ' ; 

F=norm (LOS_v- (Rb2LTP*Rc2b*RLOS2c* [1; 0; 0] ) ) ; 
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C.  GIMBALOPTIMIZATION.M 

load  g_f 

options  =  optimset (' TolX '  000001 )  ; 

s=size (g_f , 2 ) ; 

1=1; 

global  i 
for  i=l : s 

start= [g_f (2,1) , g_f (3, i) ] ; 

[x, fval , exitf lag]  =  fminunc (@ (x) gimbal (x) , start, options) 

g_i ( 1 , i ) =x ( 1 , 1 ) ; 
g_i  (2,  i)  =x  (1, 2)  ; 
f val_g ( i ) =f val ; 
i=i+l 
end 
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